import java.awt.geom.Point2D;
import java.awt.geom.Point2D.Double;
import java.util.LinkedList;
import java.util.List;
import java.util.concurrent.atomic.AtomicBoolean;

/**
 * An implementation of the Navigator Interface. This module is responsible for
 * navigating the robot along a path of real-world coordinates. Additionally
 * this implementation assigns an updater thread to feed the Chartographer of
 * "real-time" sensor data while the robot is moving. Lastly this navigator
 * makes use of the reactive module Reactor in order to detect collisions.
 * 
 * @author Emanuel YifterLindgren (dv11elr), Anders Lumio (dv11alo)
 *
 */
public class NavigationUnit2000 implements Navigator {

    protected static final long MAP_UPDATE_DELAY = 300;
    private final RobotCommunication robot;
    private final AtomicBoolean updateBlocked = new AtomicBoolean(false);
    private final LocalizationResponse lr = new LocalizationResponse();
    private final DifferentialDriveRequest dr = new DifferentialDriveRequest();
    private final LaserEchoesResponse ler = new LaserEchoesResponse();
    private final Chartographer cGrapher;
    private final Reactor reactor;
    private final double lookAhead = 0.8;

    public NavigationUnit2000(final RobotCommunication robot,
            final Chartographer cGrapher, final double[] laserAngles) {

        this.robot = robot;
        this.cGrapher = cGrapher;
        reactor = new AvoidanceReactor(laserAngles);

        // initial update
        updateMapData(lr, ler);
        // map update thread
        new Thread() {
            private final LocalizationResponse lrU = new LocalizationResponse();
            private final LaserEchoesResponse lerU = new LaserEchoesResponse();

            @Override
            public void run() {
                while (true) {
                    try {
                        Thread.sleep(MAP_UPDATE_DELAY);
                    } catch (final InterruptedException e) {
                        e.printStackTrace();
                    }
                    if (!updateBlocked.get()) {
                        updateMapData(lrU, lerU);

                    }
                }
            }
        }.start();
    }

    /**
     * Helper method that retrieves new echoes and localization from the robot
     * then sends a MapUpdate object to the Chartographer with the data.
     * 
     * @param lrU
     *            robot localization data.
     * @param lerU
     *            robot laser echoes data.
     */
    private void updateMapData(final LocalizationResponse lrU,
            final LaserEchoesResponse lerU) {

        robot.getResponse(lerU);
        robot.getResponse(lrU);

        cGrapher.updateMap(new MapUpdate(lerU.getEchoes(), lrU.getPosition(),
                lrU.getHeadingAngle()));

    }

    /*
     * (non-Javadoc)
     * 
     * @see Navigator#navigate(java.util.List)
     */
    @Override
    public void navigate(final List<Point2D.Double> path) {
        Point2D.Double pathPos;
        Point2D.Double robotPos;
        double distToPathPos;
        double linearSpeed;
        double angularSpeed;
        double headingAngle;
        double errorAngle;
        double bearingToPathPos;
        final LinkedList<Double> navPath = (LinkedList<Point2D.Double>) path;

        updateBlocked.compareAndSet(true, false);

        while (!navPath.isEmpty()) {
            robot.getResponse(lr);

            pathPos = navPath.poll();
            final double pTmp = pathPos.x;
            pathPos.x = pathPos.y;
            pathPos.y = pTmp;

            robotPos = new Point2D.Double(lr.getPosition()[0],
                    lr.getPosition()[1]);
            distToPathPos = distanceBetween(robotPos, pathPos);
            /*
             * Skip positions on the path that are closer then lookAhead
             * distance.
             */
            if (distToPathPos < lookAhead) {
                continue;
            }
            bearingToPathPos = getBearingTo(robotPos, pathPos);
            /*
             * Drive the robot towards the position until the distance is at
             * least 0.5 Units of length away from it.
             */
            do {

                robot.getResponse(lr);
                robot.getResponse(ler);

                robotPos = new Point2D.Double(lr.getPosition()[0],
                        lr.getPosition()[1]);

                headingAngle = lr.getHeadingAngle();

                errorAngle = headingAngle - bearingToPathPos;

                // check for collision and break then request new plan if
                // colliding.
                if (reactor.isCollisionDetected(robotPos, headingAngle,
                        ler.getEchoes(), navPath)) {
                    dr.setLinearSpeed(0);
                    dr.setAngularSpeed(0);
                    robot.putRequest(dr);
                    try {
                        Thread.sleep(MAP_UPDATE_DELAY);
                    } catch (final InterruptedException e) {
                        e.printStackTrace();
                    }
                    updateBlocked.compareAndSet(false, true);
                    return;
                }

                distToPathPos = distanceBetween(robotPos, pathPos);

                /*
                 * Calculate the optimal(shortest) way(left/right) for the robot
                 * to turn towards the goal.
                 */
                if (Math.toDegrees(errorAngle) > 180
                        || Math.toDegrees(errorAngle) < -180) {
                    errorAngle = -1 * errorAngle;
                }

                linearSpeed = adjustLinearSpeed(errorAngle);
                dr.setLinearSpeed(linearSpeed);
                angularSpeed = -1 * errorAngle;
                dr.setAngularSpeed(angularSpeed);

                robot.putRequest(dr);

            } while (distToPathPos >= 0.5);

            dr.setLinearSpeed(0);
            dr.setAngularSpeed(0);
            robot.putRequest(dr);

        }

        try {
            Thread.sleep(MAP_UPDATE_DELAY);
        } catch (final InterruptedException e) {
            e.printStackTrace();
        }
        updateBlocked.compareAndSet(false, true);
    }

    /**
     * Returns an appropriate linear speed given the angle to the robots goal.
     * 
     * @param errorAngle
     *            the angle in radians from the robot to its goal.
     * @return a linear speed appropriate with respect to the angle.
     */
    private double adjustLinearSpeed(final double errorAngle) {
        final double angleDeg = Math.abs(Math.toDegrees(errorAngle));
        double linSpeed;
        if (angleDeg < 5) {
            linSpeed = 0.8;
        } else if (angleDeg < 10 && angleDeg >= 5) {
            linSpeed = 0.6;
        } else if (angleDeg < 40 && angleDeg >= 10) {
            linSpeed = 0.5;
        } else if (angleDeg < 90 && angleDeg >= 40) {
            linSpeed = 0.3;
        } else if (angleDeg >= 90 && angleDeg < 120) {
            linSpeed = 0.1;
        } else {
            linSpeed = 0;
        }
        return linSpeed;
    }

    /**
     * Calculates the distance between two points.
     * 
     * @param p1
     *            first point
     * @param p2
     *            second point
     * @return the distance between the points
     */
    public double distanceBetween(final Point2D.Double p1,
            final Point2D.Double p2) {
        return Math.sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y)
                * (p2.y - p1.y));
    }

    /**
     * Gets the angle between two points.
     * 
     * @param p1
     *            first point
     * @param p2
     *            second point
     * @return the angle between p1 and p2.
     */
    public double getBearingTo(final Point2D.Double p1, final Point2D.Double p2) {
        return Math.atan2(p2.y - p1.y, p2.x - p1.x);
    }

}
